#include "robot_manager.h"
#include "utilities/file.hpp"

void RobotManager::commandJointScanCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.float64MultiArray = msg;
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::JOINT_SCAN, msg_);
    }
    cv_.notify_one();
}

void RobotManager::jointScanFunc(const CmdMsg &msg)
{
    // if (msg->data.size() == 4)
    // {
    //     ZLOGI("[Joint Scan] Received command: \nJoint ID: %.f, Joint freq: %f, Joint Amplitude: %f, Scan cycle: %f", msg->data[0], msg->data[1], msg->data[2], msg->data[3]);
    // }
    // else
    // {
    //     ZLOGW("[Joint Scan] Received command does not contain 4 values.");
    // }

    // ZLOGW("joint scan todo...");

    // std::vector<double> mit_kp, mit_kd;
    // this->get_parameter("mit_kp", mit_kp);
    // this->get_parameter("mit_kd", mit_kd);
    // std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};
    //
    // pthread_mutex_lock(&zoros_mutex_);
    // std::vector<double> init_position = joint_state_msg_.position;
    // pthread_mutex_unlock(&zoros_mutex_);
    //
    // rclcpp::Rate rate(CONTROL_FREQ);
    // double t = 0.0;
    // std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    //
    // std::vector<std::vector<double>> actual_position_sets;
    // std::vector<std::vector<double>> target_position_sets;
    // while (1)
    // {
    //     pthread_mutex_lock(&zoros_mutex_);
    //     q = joint_state_msg_.position;
    //     pthread_mutex_unlock(&zoros_mutex_);
    //     if(t > msg->data[3] / msg->data[1])
    //     {
    //         break;
    //     }
    //
    //     pthread_mutex_lock(&zoros_mutex_);
    //     robot_model_->inverseDynamics(q, qd, qdd, torque);
    //     double q_target = msg->data[2] * sin(2 * M_PI * msg->data[1] * t);
    //     double qd_target = 2 * M_PI * msg->data[1] * msg->data[2] * cos(2 * M_PI * msg->data[1] * t);
    //
    //     for (size_t i = 0; i < 7; i++)
    //     {
    //         if (!is_simulation_)
    //         {
    //             if(i == static_cast<size_t>(msg->data[0]))
    //             {
    //                 dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], q_target, qd_target, torque[i] * D[i]);
    //                 cmd_msg_.position[i] = q_target;
    //             }
    //             else
    //             {
    //                 dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], init_position[i], 0.0, torque[i] * D[i]);
    //                 cmd_msg_.position[i] = init_position[i];
    //             }
    //         }
    //         else
    //         {
    //             if(i == static_cast<size_t>(msg->data[0]))
    //             {
    //                 joint_state_msg_.position[i] = q_target;
    //             }
    //
    //         }
    //     }
    //     if(!is_simulation_)
    //     {
    //         cmd_msg_.header.stamp = this->now();
    //         cmd_pub_->publish(cmd_msg_); // 发布初始的 joint_states 消息
    //     }
    //     else
    //     {
    //         //! 仿真模式下只更新joint_state_msg_，不在这里发布消息
    //         // 避免与publishJointStatesLoop冲突
    //     }
    //     target_position_sets.push_back(cmd_msg_.position);
    //     actual_position_sets.push_back(joint_state_msg_.position);
    //     pthread_mutex_unlock(&zoros_mutex_);
    //
    //     t += 1.0 / CONTROL_FREQ;
    //     rate.sleep();
    // }
    // WriteData2D("./target.txt", target_position_sets, ',');
    // WriteData2D("./actual.txt", actual_position_sets, ',');
    // ZLOGI("scan over.");
}